/*
 * Coordinates.h
 *
 *  Created on: 23/ott/2013
 *      Author: angelo
 */

#ifndef COORDINATES_H_
#define COORDINATES_H_

#include <iostream>
#include <math.h>

using namespace std;

#ifndef EPSILON
/** @brief Constant for comparing doubles. Two doubles at most epsilon apart
 * are declared equal.*/
#define EPSILON 0.0001
#endif

class Coordinates {
private:

	/**
	 * Tests whether two doubles are close enough to be declared equal.
	 * @return true if parameters are at most epsilon apart, false
	 * otherwise
	 */
	static bool close(double one, double two) {
		return fabs(one-two)<EPSILON;
	}

public:
	 /** @brief Default constructor. */
	Coordinates()
		: x(0.0), y(0.0), z(0.0) {}

	Coordinates(double x, double y, double z = 0.0)
		: x(x), y(y), z(z) {};

	virtual ~Coordinates();

	/** @brief Adds two coordinate vectors. */
	friend Coordinates operator+(const Coordinates& a, const Coordinates& b) {
		Coordinates tmp = a;
		tmp += b;
		return tmp;
	}

	/** @brief Subtracts two coordinate vectors. */
	friend Coordinates operator-(const Coordinates& a, const Coordinates& b) {
		Coordinates tmp = a;
		tmp -= b;
		return tmp;
	}

	/** @brief Multiplies a coordinate vector by a real number.*/
	friend Coordinates operator*(const Coordinates& a, double f) {
		Coordinates tmp = a;
		tmp *= f;
		return tmp;
	}

	/** @brief Divides a coordinate vector by a real number.*/
	friend Coordinates operator/(const Coordinates& a, double f) {
		Coordinates tmp = a;
		tmp /= f;
		return tmp;
	}

	/**
	 * @brief Multiplies this coordinate vector by a real number.
	 */
	Coordinates operator*=(double f) {
		x *= f;
		y *= f;
		z *= f;
		return *this;
	}

	/**
	 * @brief Divides this coordinate vector by a real number.
	 */
	Coordinates operator/=(double f) {
		x /= f;
		y /= f;
		z /= f;
		return *this;
	}

	/**
	 * @brief Adds coordinate vector 'a' to this.
	 */
	Coordinates operator+=(const Coordinates& a) {
		x += a.x;
		y += a.y;
		z += a.z;
		return *this;
	}

	/**
	 * @brief Assigns coordinate vector 'a' to this.
	 *
	 * This operator can change the dimension of the coordinate.
	 */
	Coordinates operator=(const Coordinates& a) {
		x = a.x;
		y = a.y;
		z = a.z;
		return *this;
	}

	/**
	 * @brief Subtracts coordinate vector 'a' from this.
	 */
	Coordinates operator-=(const Coordinates& a) {
		x -= a.x;
		y -= a.y;
		z -= a.z;
		return *this;
	}

	/**
	 * @brief Tests whether two coordinate vectors are equal.
	 *
	 * Because coordinates are of type double, this is done through the
	 * FWMath::close function.
	 */
	friend bool operator==(const Coordinates& a, const Coordinates& b) {
		return close(a.x, b.x) && close(a.y, b.y) && close(a.z, b.z);
	}

	/**
	 * @brief Tests whether two coordinate vectors are not equal.
	 *
	 * Negation of the operator==.
	 */
	friend bool operator!=(const Coordinates& a, const Coordinates& b) {
		return !(a==b);
	}

	/**
	 * @brief Returns the distance to Coord 'a'.
	 */
	double distance( const Coordinates& a ) const {
		Coordinates dist=*this-a;
		return dist.length();
	}

	/**
	 * @brief Returns distance^2 to Coord 'a' (omits calling square root).
	 */
	double sqrdist( const Coordinates& a ) const {
		Coordinates dist=*this-a;
		return dist.squareLength();
	}

	/**
	 * @brief Returns the squared distance on a torus of this to Coord 'b' (omits calling square root).
	 */
	double sqrTorusDist(const Coordinates& b, const Coordinates& size) const;

	/**
	 * @brief Returns the square of the length of this Coords position vector.
	 */
	double squareLength() const
	{
		return x * x + y * y + z * z;
	}

	/**
	 * @brief Returns the length of this Coords position vector.
	 */
	double length() const
	{
		return sqrt(squareLength());
	}

	/**
	 * @brief Checks if this coordinate is inside a specified rectangle.
	 *
	 * @param lowerBound The upper bound of the rectangle.
	 * @param upperBound The lower bound of the rectangle.
	 */
	bool isInBoundary(const Coordinates& lowerBound, const Coordinates& upperBound) const {
		return  lowerBound.x <= x && x <= upperBound.x &&
				lowerBound.y <= y && y <= upperBound.y &&
				lowerBound.z <= z && z <= upperBound.z;
	}

	/**
	 * @brief Returns the minimal coordinates.
	 */
	Coordinates min(const Coordinates& a) {
		return Coordinates(this->x < a.x ? this->x : a.x,
				this->y < a.y ? this->y : a.y,
						this->z < a.z ? this->z : a.z);
	}

	/**
	 * @brief Returns the maximal coordinates.
	 */
	Coordinates max(const Coordinates& a) {
		return Coordinates(this->x > a.x ? this->x : a.x,
				this->y > a.y ? this->y : a.y,
						this->z > a.z ? this->z : a.z);
	}

	/**
	 * @brief Calculate the middle point between two points
	 */
	Coordinates midPoint(Coordinates point) {
		Coordinates ris = Coordinates::ZERO;
		ris.x = (this->x + point.x) / 2.0;
		ris.y = (this->y + point.y) / 2.0;
		return ris;
	}

public:
	/** @brief Constant with all values set to 0. */
	static const Coordinates ZERO;

	static double calculateAngle(Coordinates a, Coordinates b, Coordinates c) {
		double vABx, vABy, vABz, vCBx, vCBy, vCBz, tmp1,tmp2;

		vABx = a.x - b.x;
		vABy = a.y - b.y;
		vABz = a.z - b.z;
		vCBx = c.x - b.x;
		vCBy = c.y - b.y;
		vCBz = c.z - b.z;

		tmp1 = vABx*vCBx + vABy*vCBy + vABz*vCBz;
		tmp2 = sqrt(((vABx*vABx) + (vABy*vABy) + (vABz*vABz)) * ((vCBx*vCBx) + (vCBy*vCBy) + (vCBz*vCBz)));

		if(tmp2 != 0) {
			return acos(tmp1/tmp2);
		}
		else {
			return 0;
		}
	}

	static void cartesian2polar(double x, double y, double &r, double &theta) {
		r = hypot(x, y);
		theta = atan2(y, x);
	}

	static void polar2cartesian(double r, double theta, double &x, double &y) {
		x = r * cos(theta);
		y = r * sin(theta);
	}

	static void calculateLine(Coordinates a, Coordinates b, double &y, double &x, double &c) {
		if (close(a.y, b.y)) {
			x = 0;
			y = 1;
			c = -a.y;
		}
		else if (close(a.x, b.x)) {
			x = 1;
			y = 0;
			c = -a.x;
		}
		else {
			x = -((b.y - a.y) / (b.x - a.x));
			y = 1;
			c = ((a.x * (b.y - a.y)) / (b.x - a.x)) - a.y;
		}
	}

	static Coordinates calculateIntersection (double y1, double x1, double c1, double y2, double x2, double c2) {
		Coordinates ris = Coordinates::ZERO;

		if (y1 == 0) {
			ris.x = -(c1 / x1);
			ris.y = ((x2*c1) - (c2*x1)) / (x1 * y2);
		}
		else {
			ris.x = ((y2*c1) - (y1*c2)) / ((y1*x2) - (y2*x1));
			ris.y = (-(ris.x*x1) - c1) / y1;
		}

		return ris;
	}

	static Coordinates getequilateralPoint(Coordinates a, Coordinates b, Coordinates c) {
		Coordinates ab1, ab2, a_b;
		double a_b_r, a_b_t;

		a_b = b - a;
		Coordinates::cartesian2polar(a_b.x, a_b.y, a_b_r, a_b_t);

		// provo ad aggiungere e togliere 60°
		polar2cartesian(a_b_r, a_b_t + (M_PI / 3), ab1.x, ab1.y);
		polar2cartesian(a_b_r, a_b_t - (M_PI / 3), ab2.x, ab2.y);

		ab1 = ab1 + a;
		ab2 = ab2 + a;

		if(c.distance(ab1) > c.distance(ab2)) {
			return ab1;
		}
		else {
			return ab2;
		}
	}

	static Coordinates calculateSteinerPoint (Coordinates a, Coordinates b, Coordinates c) {
		Coordinates ris = Coordinates::ZERO;
		Coordinates ab, ac, bc;

		double x1, x2, x3, y1, y2, y3, c1, c2, c3;

		Coordinates p1, p2, p3;

		//calcolo il punto del triangolo equilatero tra due punti, opposto al terzo punto
		ab = Coordinates::getequilateralPoint(a, b, c);
		ac = Coordinates::getequilateralPoint(a, c, b);
		bc = Coordinates::getequilateralPoint(b, c, a);

		// calcolo le rette
		Coordinates::calculateLine(ab, c, y1, x1, c1);	// retta ab-c
		Coordinates::calculateLine(ac, b, y2, x2, c2);	// retta ac-b
		Coordinates::calculateLine(bc, a, y3, x3, c3);	// retta bc-a

		// calcolo l'intersezione tra le rette
		p1 = Coordinates::calculateIntersection(y1, x1, c1, y2, x2, c2); // calcolo l'intersezione ab-c con ac-b
		p2 = Coordinates::calculateIntersection(y1, x1, c1, y3, x3, c3); // calcolo l'intersezione ab-c con bc-a
		p3 = Coordinates::calculateIntersection(y3, x3, c3, y2, x2, c2); // calcolo l'intersezione bc-a con ac-b

		if((p1 == p2) && (p1 == p3)) {
			// effettuo il controllo per sicurezza
			ris = p1;
			//cout << "OK!!! -> " << ris << endl;
		}

		return ris;
	}

public:
    /** @name x, y and z coordinate of the position. */
    /*@{*/
    double x;
    double y;
    double z;
    /*@}*/
};

inline std::ostream& operator<<(std::ostream& os, const Coordinates& coord)
{
    return os << "(" << coord.x << "," << coord.y << "," << coord.z << ")";
}


#endif /* COORDINATES_H_ */
